#include <master.h>
#include <sched.h>
#include <debug.h>

///
// sched.c - scheduler code
//
// Portions of this code contributed by Napalm (napalm@netcore2k.net)
//



thread_list_t *currentthread,	// currently running thread 
	      *readyqueue, 	// threads ready to run
	      *waitqueue, 	// threads waiting
	      *killqueue;	// threads to be reaped

unsigned long global_tid = 1;

bool sched_initialized = FALSE;

void sched_install()
{
	// initial thread
	readyqueue = kmalloc_stupid(sizeof(thread_list_t));
	memset(readyqueue, 0, sizeof(thread_list_t));

	killqueue = NULL;
	waitqueue = NULL;

	readyqueue->quantum = 		SCHED_QUANTUM;
	readyqueue->next =		readyqueue;
	readyqueue->prev = 		readyqueue;

	currentthread = readyqueue;

	sched_initialized = TRUE;
}

static void sched_insert(thread_list_t *list, thread_list_t *node) {
     node->next = list->next;
     node->prev = list;

     (list->next)->prev = node;
     list->next = node;
}

static void sched_remove(thread_list_t *t) {
     if (t->next == t) panic("removing the last thread!");

     (t->next)->prev = t->prev;
     (t->prev)->next = t->next;
}

static void list_dump(thread_list_t *head) {
  thread_list_t *iter = head;
  dprint("Head is %p\n", head);

  do {
   dprint("(%d -> %d) <- Thread %d -> (%d <- %d)\n", iter->prev->tid, iter->prev->next->tid, 
						     iter->tid, 
						     iter->next->prev->tid, iter->next->tid);

   if ((iter->prev)->next != iter) {
    dprint("Thread %d is not thread %d (prev)\n", (iter->prev)->next->tid, iter->tid);
    panic("list is fucked");
   }

   if ((iter->next)->prev != iter) { 
    dprint("Thread %d is not thread %d (next)\n", (iter->next)->prev->tid, iter->tid);
    panic("list is fucked");
   }
   if (iter->next == head) {
     dprint("Closed loop.\n");
     return;
   } 
  } while (iter=iter->next);
   dprint("Thread %d, address %p, next %p\n", iter->tid, iter, iter->next);
  dprint("Open loop.\n");
}

unsigned int sched_contextswitch(unsigned int oldesp) // simple round robin schedule
{
	thread_list_t *killme;

	if(!sched_initialized) return oldesp;

	currentthread->esp = oldesp;

	////dprint("Thread %d,  ESP %p\n", sched_threadid, currentthread->esp);

	if (killqueue) {
	  dprint("Reaping dead threads.\n");
	  while(killqueue) {
	   dprint("Freeing thread id %d at %p\n", killqueue->tid, killqueue);
           kfree(killqueue->stack);
           killme = killqueue->next;
	   kfree(killqueue);
	   killqueue = killme;
 	  }
 	} 

//	dprint("In scheduler, readyqueue is %p\n", readyqueue);

	if (currentthread->quantum == 0) { 
		//dprint("Switch thread %d to %d\n", currentthread->tid, readyqueue->next->tid);

		#if 0
		dprint("{%16d} %d (%d) -> [%d (%d)] -> %d (%d)\n", 
					       timer_ticks, 
					       currentthread->prev->tid, 
					       currentthread->prev->quantum, 
					       currentthread->tid,
					       currentthread->quantum,
					       currentthread->next->tid,
					       currentthread->next->quantum );
		#endif

		currentthread->quantum = SCHED_QUANTUM;
	
		currentthread = readyqueue->next;
		readyqueue = readyqueue->next;
        }
	currentthread->quantum--;
	
	//dprint("Current thread about to be run is %d - %d ticks left\n", currentthread->tid, currentthread->quantum);

	return currentthread->esp;
}

bool sched_createthread(startaddress start, void *argument)
{
	disable_irqs();

	thread_list_t *newthread = kmalloc_stupid(sizeof(thread_list_t));
	newthread->start   = start;
	newthread->quantum = SCHED_QUANTUM;
	newthread->prio    = 65535;
	newthread->expiry  = 0;

	newthread->tid = global_tid++;

	// Allocating space for a 4kb stack
        newthread->stack = (dword *)((dword)kmalloc_stupid(4096) + 4096);
	dprint("Allocated a kernel thread stack at %p\n", newthread->stack);
	dword *stackptr = newthread->stack;
	
	// Push all registers to the stack
        *--stackptr    = (unsigned int)argument;		// ARGUMENT TO THREAD
	*--stackptr    = (unsigned int)sched_endthread;		// RETURN ADDRESS

	*--stackptr    = 0x0202;				// eflags --+
	*--stackptr    = 0x08;					// cs       +-- pop by cpu
	*--stackptr    = (dword)start;				// eip -----+
	*--stackptr    = 0;						// ebp -----+
	*--stackptr    = 0;						// esp      |
	*--stackptr    = 0;						// edi      |
	*--stackptr    = 0;						// esi      +-- popa
	*--stackptr    = 0;						// edx      |
	*--stackptr    = 0;						// ecx      |
	*--stackptr    = 0;						// ebx      |
	*--stackptr    = 0;						// eax -----+
	*--stackptr    = 0x10;					// ds
	*--stackptr    = 0x10;					// es
	*--stackptr    = 0x10;					// fs
	*--stackptr    = 0x10;					// gs

	newthread->esp = (unsigned int)stackptr;
	
	dprint("New thread id %d base: %p esp: %p\n", newthread->tid, newthread->stack, newthread->esp);

	sched_insert(readyqueue->prev, newthread); // insert before instead of after
         
	if (readyqueue->next == readyqueue) readyqueue->next = newthread;

	list_dump(readyqueue);

	enable_irqs();
	return 1;
}



void sched_killthread(thread_list_t *thread) {
	disable_irqs(); // safety

	dprint("Killing thread id %d at %p\n", thread->tid, thread);
                
	thread_list_t **killme = &killqueue, **last = NULL;

	// in case
	thread->quantum = 0;

	sched_remove(thread);

	// -- BIG BUGFIX :D --
	readyqueue = readyqueue->next;

	dprint("Removed thread from queue\n");

	// see if we need to add to the killqueue
	while(*killme) {
	  dprint("Kill queue is %p\n", *killme);
	  last = killme;	     // save node
	  killme = &(*killme)->next; // iterate over the kill queue
				     // to find last node
	}
	
	*killme = thread; 
	if (last) (*last)->next = thread;
	thread->next = NULL;
	thread->prev = NULL;

	dprint("Thread attached to kill queue.\n");

	enable_irqs();
}


void sched_endthread() {
	dprint("Kill current thread %d.\n", currentthread->tid);
	sched_killthread(currentthread);
	dprint("Pausing.\n");

	// PAUSE opcode, lol.
	while(1) 
           __asm__ __volatile__ ("rep; nop");
      
}
